
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms.h
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mode.h"

#include "define.h"
#include "config.h"

#include <stdint.h>
#include <stdbool.h>

#include <ahrs/ahrs_view.h>
#include <common/microbee.h>
#include <mission/mb_mission.h>
#include <rc_mapper/rc_mapper.h>
#include <common/time/gp_time.h>
#include <vehicle/mb_fixed_wing.h>
#include <rc_channel/rc_channel.h>
#include <mb_landing/mb_landing.h>
#include <srv_channel/srv_channel.h>
#include <common/location/location.h>
#include <mb_navigation/mb_navigation.h>
#include <mp_control/mp_roll_controller.h>
#include <mp_control/mp_pitch_controller.h>
#include <mp_control/mp_yaw_controller.h>
#include <mp_control/mp_steer_controller.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum Landing_ApproachStage {
    LAS_RTL,
    LAS_LOITER_TO_ALT,
    LAS_ENSURE_RADIUS,
    LAS_WAIT_FOR_BREAKOUT,
    LAS_APPROACH_LINE,
    LAS_VTOL_LANDING,
};

enum Failsafe_Action {
    Failsafe_Action_None      = 0,
    Failsafe_Action_RTL       = 1,
    Failsafe_Action_Land      = 2,
    Failsafe_Action_Terminate = 3,
#if HAL_QUADPLANE_ENABLED
    Failsafe_Action_QLand     = 4,
#endif
    Failsafe_Action_Parachute = 5,
#if HAL_QUADPLANE_ENABLED
    Failsafe_Action_Loiter_alt_QLand = 6,
#endif
};

enum ThrFailsafe {
    TFS_Disabled    = 0,
    TFS_Enabled     = 1,
    TFS_EnabledNoFS = 2
};

enum FlareMode {
    FM_FLARE_DISABLED = 0,
    FM_ENABLED_NO_PITCH_TARGET,
    FM_ENABLED_PITCH_TARGET
};

typedef struct fms_handle* fms_handle_t;
typedef const struct fms_handle* fms_handle_const_t;

struct fms_handle {
    ahrs_view *ahrs;

    struct mb_fixed_wing aparm;

    // IMU variables
    // Integration time; time last loop took to run
    float G_Dt;
    float FAST_Dt;

    struct {
        Param_int16        format_version;

        // Flight modes
        //
        Param_int8 flight_mode_channel;
        Param_int8 flight_modes[6];
        Param_int8 initial_mode;

        Param_int8 rangefinder_landing;
        Param_int8 stick_mixing;

        // Throttle
        //
        Param_int8  throttle_suppress_manual;
        Param_int8  throttle_passthru_stabilize;
        Param_int8  throttle_fs_enabled;
        Param_int16 throttle_fs_value;
        Param_int8  throttle_nudge;
        Param_int32 use_reverse_thrust;

        // Failsafe
        Param_int8  fs_action_short;
        Param_int8  fs_action_long;
        Param_float fs_timeout_short;
        Param_float fs_timeout_long;
        Param_int8  gcs_heartbeat_fs_enabled;

        Param_int8  man_expo_roll;
        Param_int8  man_expo_pitch;
        Param_int8  man_expo_rudder;

        // Misc
        //
        Param_int8  rudder_only;
        Param_float mixing_gain;
        Param_float mixing_offset;
        Param_int16 dspoiler_rud_rate;

        // home reset altitude threshold
        Param_int8 home_reset_threshold;

        Param_int32 flight_options;
        Param_int8  rtl_autoland;

        // dual motor tailsitter rudder to differential thrust scaling: 0-100%
        Param_int8 rudd_dt_gain;

        Param_float takeoff_throttle_min_speed;
        Param_float takeoff_throttle_min_accel;
        Param_int8 takeoff_throttle_delay;
        Param_int8 takeoff_tdrag_elevator;
        Param_float takeoff_tdrag_speed1;
        Param_float takeoff_rotate_speed;
        Param_int8 takeoff_throttle_slewrate;
        Param_float takeoff_pitch_limit_reduction_sec;
        Param_int8 takeoff_flap_percent;

        // crow flaps weighting
        Param_int8 crow_flap_weight_outer;
        Param_int8 crow_flap_weight_inner;
        Param_int8 crow_flap_options;
        Param_int8 crow_flap_aileron_matching;

        // mask of channels to do manual pass-thru for
        Param_int32 manual_rc_mask;

        // Forward throttle battery voltage compenstaion
        Param_float fwd_thr_batt_voltage_max;
        Param_float fwd_thr_batt_voltage_min;
        Param_int8  fwd_thr_batt_idx;

        // Navigational maneuvering limits
        //
        Param_int16 alt_offset;
        Param_int16 acro_roll_rate;
        Param_int16 acro_pitch_rate;
        Param_int16 acro_yaw_rate;
        Param_int8  acro_locking;

        Param_int8 flap_1_percent;
        Param_int8 flap_1_speed;
        Param_int8 flap_2_percent;
        Param_int8 flap_2_speed;

        Param_int8 flap_slewrate;
        Param_int8 crash_accel_threshold;

        Param_float kff_rudder_mix;
        Param_float kff_throttle_to_pitch;
        Param_float ground_steer_alt;
        Param_int16 ground_steer_dps;
        Param_float stab_pitch_down;
    } g;

    // primary input control channels
    rc_channel_t channel_roll;
    rc_channel_t channel_pitch;
    rc_channel_t channel_throttle;
    rc_channel_t channel_rudder;
    rc_channel_t channel_flap;
    rc_channel_t channel_airbrake;

    // scaled roll limit based on pitch
    int32_t roll_limit_cd;
    int32_t pitch_limit_min_cd;

    // flight modes convenience array
    Param_int8 *flight_modes;
    uint8_t num_flight_modes;

    struct rangefinder_state rangefinder_state;

    // Attitude to servo controllers
    struct mp_roll_controller roll_controller;
    struct mp_pitch_controller pitch_controller;
    struct mp_yaw_controller yaw_controller;
    struct mp_steer_controller steer_controller;

    // selected navigation controller
    navctrl_t nav_controller;

    // Training mode
    bool training_manual_roll;  // user has manual roll control
    bool training_manual_pitch; // user has manual pitch control

    // should throttle be pass-thru in guided?
    bool guided_throttle_passthru;

    // are we doing calibration? This is used to allow heartbeat to
    // external failsafe boards during baro and airspeed calibration
    bool in_calibration;

    struct ModeInitializing mode_initializing;

    struct ModeManual       mode_manual;
    struct ModeAuto         mode_auto;
    struct ModeFBWA         mode_fbwa;
    struct ModeFBWB         mode_fbwb;
    struct ModeTakeoff      mode_takeoff;
    struct ModeStabilize    mode_stabilize;
    struct ModeTraining     mode_training;
    struct ModeAcro         mode_acro;
    struct ModeAutoTune     mode_autotune;
    struct ModeGuided       mode_guided;
    struct ModeCruise       mode_cruise;
    struct ModeLoiter       mode_loiter;

    // This is the state of the flight control system
    // There are multiple states defined such as MANUAL, FBW-A, AUTO
    mode_base_t control_mode;
    mode_base_t previous_mode;

    ModeReason control_mode_reason;

    // mode reason for entering previous mode
    ModeReason previous_mode_reason;

    // time of last mode change
    uint32_t last_mode_change_ms;

    // This is used to enable the inverted flight feature
    bool inverted_flight;

    // last time we ran roll/pitch stabilization
    uint32_t last_stabilize_ms;

    // Failsafe
    struct {
        // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
        // RC receiver should be set up to output a low throttle value when signal is lost
        bool rc_failsafe;

        // true if an adsb related failsafe has occurred
        bool adsb;

        // saved flight mode
        enum ModeNumber saved_mode_number;

        // A tracking variable for type of failsafe active
        // Used for failsafe based on loss of RC signal or GCS signal
        int16_t state;

        // number of low throttle values
        uint8_t throttle_counter;

        // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
        uint32_t short_timer_ms;

        uint32_t last_valid_rc_ms;

        //keeps track of the last valid rc as it relates to the AFS system
        //Does not count rc inputs as valid if the standard failsafe is on
        uint32_t AFS_last_valid_rc_ms;
    } failsafe;

#if HAL_QUADPLANE_ENABLED
    // Landing
    struct {
        enum Landing_ApproachStage approach_stage;
        float approach_direction_deg;
    } vtol_approach_s;
#endif

    // A counter used to count down valid gps fixes to allow the gps estimate to settle
    // before recording our home position (and executing a ground start if we booted with an air start)
    uint8_t ground_start_count;

    // true if we have a position estimate from AHRS
    bool have_position;

    // Airspeed
    // The calculated airspeed to use in FBW-B.  Also used in higher modes for insuring min ground speed is met.
    // Also used for flap deployment criteria.  Centimeters per second.
    int32_t target_airspeed_cm;
    int32_t new_airspeed_cm;  //temp variable for AUTO and GUIDED mode speed changes

    // The difference between current and desired airspeed.  Used in the pitch controller.  Meters per second.
    float airspeed_error;

    // An amount that the airspeed should be increased in auto modes based on the user positioning the
    // throttle stick in the top half of the range.  Centimeters per second.
    int16_t airspeed_nudge_cm;

    // Similar to airspeed_nudge, but used when no airspeed sensor.
    // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
    int16_t throttle_nudge;

    // Ground speed
    // The amount current ground speed is below min ground speed.  Centimeters per second
    int32_t groundspeed_undershoot;

    // Difference between current altitude and desired altitude.  Centimeters
    int32_t altitude_error_cm;

    // speed scaler for control surfaces, updated at 10Hz
    float surface_speed_scaler;

    struct {
        uint32_t last_tkoff_arm_time;
        uint32_t last_check_ms;
        uint32_t rudder_takeoff_warn_ms;
        uint32_t last_report_ms;
        bool launchTimerStarted;
        uint8_t accel_event_counter;
        uint32_t accel_event_ms;
        uint32_t start_time_ms;
        bool waiting_for_rudder_neutral;
    } takeoff_state;

    // ground steering controller state
    struct {
        // Direction held during phases of takeoff and landing centidegrees
        // A value of -1 indicates the course has not been set/is not in use
        // this is a 0..36000 value, or -1 for disabled
        int32_t hold_course_cd;

        // locked_course and locked_course_cd are used in stabilize mode
        // when ground steering is active, and for steering in auto-takeoff
        bool locked_course;
        float locked_course_err;
        uint32_t last_steer_ms;
    } steer_state;

    // flight mode specific
    struct {
        // Altitude threshold to complete a takeoff command in autonomous
        // modes.  Centimeters above home
        int32_t takeoff_altitude_rel_cm;

        // Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
        int32_t height_below_takeoff_to_level_off_cm;

        // the highest airspeed we have reached since entering AUTO. Used
        // to control ground takeoff
        float highest_airspeed;

        // turn angle for next leg of mission
        float next_turn_angle;

        // filtered sink rate for landing
        float sink_rate;

        // time when we first pass min GPS speed on takeoff
        uint32_t takeoff_speed_time_ms;

        // distance to next waypoint
        float wp_distance;

        // proportion to next waypoint
        float wp_proportion;

        // last time is_flying() returned true in milliseconds
        uint32_t last_flying_ms;

        // time stamp of when we start flying while in auto mode in milliseconds
        uint32_t started_flying_in_auto_ms;

        // barometric altitude at start of takeoff
        float baro_takeoff_alt;

        // initial pitch. Used to detect if nose is rising in a tail dragger
        int16_t initial_pitch_cd;

        // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
        int16_t takeoff_pitch_cd;

        // used to 'wiggle' servos in idle mode to prevent them freezing
        // at high altitudes
        uint8_t idle_wiggle_stage;

        // Flag for using gps ground course instead of INS yaw.  Set false when takeoff command in process.
        bool takeoff_complete;

        // are we headed to the land approach waypoint? Works for any nav type
        bool wp_is_land_approach;

        // should we fly inverted?
        bool inverted_flight;

        // should we enable cross-tracking for the next waypoint?
        bool next_wp_crosstrack;

        // should we use cross-tracking for this waypoint?
        bool crosstrack;

        // in FBWA taildragger takeoff mode
        bool fbwa_tdrag_takeoff_mode;

        // have we checked for an auto-land?
        bool checked_for_autoland;

        // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
        // are we in idle mode? used for balloon launch to stop servo
        // movement until altitude is reached
        bool idle_mode;

        // are we in VTOL mode in AUTO?
        bool vtol_mode;

        // are we doing loiter mode as a VTOL?
        bool vtol_loiter;

        // how much correction have we added for terrain data
        float terrain_correction;
    } auto_state;

    struct {
        // roll pitch yaw commanded from external controller in centidegrees
        int32_t forced_rpy_cd[3];
        // last time we heard from the external controller
        int32_t last_forced_rpy_ms[3];

        // throttle  commanded from external controller in percent
        float forced_throttle;
        uint32_t last_forced_throttle_ms;

        // airspeed adjustments
        float target_airspeed_cm;  // don't default to zero here, as zero is a valid speed.
        float target_airspeed_accel;
        uint32_t target_airspeed_time_ms;

        // altitude adjustments
        float target_alt;   // don't default to zero here, as zero is a valid alt.
        uint32_t last_target_alt;
        float target_alt_accel;
        uint32_t target_alt_time_ms;
        uint8_t target_alt_frame;

        // heading track
        float target_heading; // don't default to zero or -1 here, as both are valid headings in radians
        float target_heading_accel_limit;
        uint32_t target_heading_time_ms;
        enum guided_heading_type_t target_heading_type;
        bool target_heading_limit_low;
        bool target_heading_limit_high;
    } guided_state;

    struct {
        // on hard landings, only check once after directly a landing so you
        // don't trigger a crash when picking up the aircraft
        bool checkedHardLanding;

        // crash detection. True when we are crashed
        bool is_crashed;

        // impact detection flag. Expires after a few seconds via impact_timer_ms
        bool impact_detected;

        // debounce timer
        uint32_t debounce_timer_ms;

        // delay time for debounce to count to
        uint32_t debounce_time_total_ms;

        // length of time impact_detected has been true. Times out after a few seconds. Used to clip isFlyingProbability
        uint32_t impact_timer_ms;
    } crash_state;

    // this controls throttle suppression in auto modes
    bool throttle_suppressed;

    // reduce throttle to eliminate battery over-current
    int8_t  throttle_watt_limit_max;
    int8_t  throttle_watt_limit_min; // for reverse thrust
    uint32_t throttle_watt_limit_timer_ms;

    enum FlightStage flight_stage;

    // probability of aircraft is currently in flight. range from 0 to
    // 1 where 1 is 100% sure we're in flight
    float isFlyingProbability;

    // previous value of is_flying()
    bool previous_is_flying;

    // time since started flying in any mode in milliseconds
    uint32_t started_flying_ms;

    // ground mode is true when disarmed and not flying
    bool ground_mode;

    // Navigation control variables
    // The instantaneous desired bank angle.  Hundredths of a degree
    int32_t nav_roll_cd;

    // The instantaneous desired pitch angle.  Hundredths of a degree
    int32_t nav_pitch_cd;

    // the aerodymamic load factor. This is calculated from the demanded
    // roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
    float aerodynamic_load_factor;

    // a smoothed airspeed estimate, used for limiting roll angle
    float smoothed_airspeed;

    /*
      meta data to support counting the number of circles in a loiter
    */
    struct {
        // previous target bearing, used to update sum_cd
        int32_t old_target_bearing_cd;

        // Total desired rotation in a loiter.  Used for Loiter Turns commands.
        int32_t total_cd;

        // total angle completed in the loiter so far
        int32_t sum_cd;

        // Direction for loiter. 1 for clockwise, -1 for counter-clockwise
        int8_t direction;

        // when loitering and an altitude is involved, this flag is true when it has been reached at least once
        bool reached_target_alt;

        // check for scenarios where updrafts can keep you from loitering down indefinitely.
        bool unable_to_acheive_target_alt;

        // start time of the loiter.  Milliseconds.
        uint32_t start_time_ms;

        // altitude at start of loiter loop lap. Used to detect delta alt of each lap.
        // only valid when sum_cd > 36000
        int32_t start_lap_alt_cm;
        int32_t next_sum_lap_cd;

        // The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
        uint32_t time_max_ms;
    } loiter;

    // Conditional command
    // A value used in condition commands (eg delay, change alt, etc.)
    // For example in a change altitude command, it is the altitude to change to.
    int32_t condition_value;

    // A starting value used to check the status of a conditional command.
    // For example in a delay command the condition_start records that start time for the delay
    uint32_t condition_start;
    // A value used in condition commands.  For example the rate at which to change altitude.
    int16_t condition_rate;

    // 3D Location vectors
    // Location structure defined in AP_Common
    const Location *home;

    // The location of the previous waypoint.  Used for track following and altitude ramp calculations
    Location prev_WP_loc;

    // The plane's current location
    Location current_loc;

    // The location of the current/active waypoint.  Used for altitude ramp, track following and loiter calculations.
    Location next_WP_loc;

    // Altitude control
    struct {
        // target altitude above sea level in cm. Used for barometric
        // altitude navigation
        int32_t amsl_cm;

        // Altitude difference between previous and current waypoint in
        // centimeters. Used for glide slope handling
        int32_t offset_cm;

#if 0
        // are we trying to follow terrain?
        bool terrain_following;

        // target altitude above terrain in cm, valid if terrain_following
        // is set
        int32_t terrain_alt_cm;

        // lookahead value for height error reporting
        float lookahead;
#endif

        // last input for FBWB/CRUISE height control
        float last_elevator_input;

        // last time we checked for pilot control of height
        uint32_t last_elev_check_us;
    } target_altitude;

    float relative_altitude;

    struct {
        uint32_t last_trim_check;
        uint32_t last_trim_save;
    } auto_trim;

    struct {
        bool done_climb;
    } rtl;

    // last time home was updated while disarmed
    uint32_t last_home_update_ms;

    // time that rudder arming has been running
    uint32_t rudder_arm_timer;

    // have we seen neutral rudder since arming with rudder?
    bool seen_neutral_rudder;

#if HAL_QUADPLANE_ENABLED
    // support for quadcopter-plane
    QuadPlane quadplane{ahrs};
#endif

    // rudder mixing gain for differential thrust (0 - 1)
    float rudder_dt;

    // soaring mode-change timer
    uint32_t soaring_mode_timer_ms;

    // terrain disable for non AUTO modes, set with an RC Option switch
    bool non_auto_terrain_disable;

    // reverse_thrust.cpp
    bool reversed_throttle;
    bool have_reverse_throttle_rc_option;

    // last target alt we passed to tecs
    int32_t tecs_target_alt_cm;

    enum CrowMode crow_mode;
    enum FlareMode flare_mode;

    bool autotuning;
};

/*----------------------------------variable----------------------------------*/
extern struct fms_handle fms;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
gp_err task_fms_init(void);

void fms_ctor();

///Param
void fms_assign_param();

/// reverse_thrust
bool fms_have_reverse_thrust(void);
bool fms_allow_reverse_thrust(void);
float fms_get_throttle_input(bool no_deadzone);
float fms_get_adjusted_throttle_input(bool no_deadzone);

///AHRS
void fms_update_ahrs();
void fms_update_current_loc(void);
void fms_ahrs_init();

/// fms_event
void fms_handle_battery_failsafe(const char *type_str, const int8_t action);

/// FMS GCS
void fms_gcs_init();
void fms_gcs_update();
void fms_handle_common_mission_message(mavlink_message_t *msg, uint8_t chan);

/// fms_commands
bool fms_update_home();
bool fms_set_home_persistently(const Location *loc);

/// fms_state
bool fms_any_failsafe_triggered();
void fms_publish_vehicle_hearbeat();
void fms_actuator_armed_notify(bool b);
void fms_publish_vehicle_vfr_hud();

/// is_flying
void fms_update_is_flying_5Hz(void);
void fms_crash_detection_update(void *uitc_gps);
bool fms_is_flying(void);
bool fms_in_preLaunch_flight_stage(void);

/// Radio
bool fms_rc_throttle_value_ok(void);
bool fms_rc_failsafe_active(void);
float fms_roll_in_expo(bool use_dz);
float fms_pitch_in_expo(bool use_dz);
float fms_rudder_in_expo(bool use_dz);
void fms_set_control_channels(void);
void fms_init_rc_in();
void fms_init_rc_out_main();
void fms_init_rc_out_aux();
void fms_rudder_arm_disarm_check();
void fms_read_radio();
void fms_control_failsafe();
bool fms_throttle_at_zero(void);
int16_t fms_rudder_input(void);

/// Mode
bool fms_set_mode(mode_base_t new_mode, const ModeReason reason);
bool fms_set_mode2(const uint8_t new_mode, const ModeReason reason);
bool fms_set_mode_by_number(enum ModeNumber new_mode_number, const ModeReason reason);
mode_base_t fms_mode_from_mode_num(const enum ModeNumber num);
bool fms_fly_inverted(void);
bool fms_flight_option_enabled(enum FlightOptions flight_option);
void fms_update_control_mode(void);
void fms_update_fly_forward(void);

/// commands_logic
bool start_command_callback(mission_command_const_t cmd);
bool verify_command_callback(mission_command_const_t cmd);
void exit_mission_callback();

/// fms_servos
void fms_set_servos(void);
void fms_set_servos_idle(void);
void fms_set_servos_controlled(void);
void fms_throttle_voltage_comp(int8_t *min_throttle, int8_t *max_throttle);
void fms_throttle_watt_limiter(int8_t *min_throttle, int8_t *max_throttle);
bool fms_suppress_throttle(void);
void fms_set_servos_flaps(void);
void fms_flaperon_update();
void fms_airbrake_update(void);
void fms_servos_output(void);
void fms_servos_twin_engine_mix(void);
void fms_throttle_slew_limit(Aux_servo_function_t func);
void fms_channel_function_mixer(Aux_servo_function_t func1_in, Aux_servo_function_t func2_in,
                                   Aux_servo_function_t func1_out, Aux_servo_function_t func2_out);
void fms_force_flare(void);
void fms_dspoiler_update(void);
void fms_landing_neutral_control_surface_servos(void);
void fms_indicate_waiting_for_rud_neutral_to_takeoff(void);
void fms_servos_auto_trim(void);

/// fms_altitude
int32_t fms_adjusted_relative_altitude_cm(void);
float fms_mission_alt_offset(void);
void fms_reset_offset_altitude(void);

/// fms_attitude
void fms_update_load_factor(void);
void fms_adjust_nav_pitch_throttle(void);
int8_t fms_throttle_percentage(void);
void fms_stabilize();
void fms_stabilize_stick_mixing_fbw();
bool fms_stick_mixing_enabled(void);
void fms_stabilize_roll();
float fms_stabilize_roll_get_roll_out();
void fms_stabilize_pitch();
float fms_stabilize_pitch_get_pitch_out();
void fms_stabilize_yaw();
int16_t fms_calc_nav_yaw_course(void);
int16_t fms_calc_nav_yaw_ground(void);
int16_t fms_calc_nav_yaw_coordinated();

/// fms_takeoff
int8_t fms_takeoff_tail_hold(void);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



